#include "robot.h"

#include <arm.h>
#include <chassis.h>
#include <ins_task.h>
#include <mastermachine.h>

#include "motor_task.h"
#include "command.h"

#include "bsp_init.h"

#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"
#include "bsp_usart.h"

static void OSTaskInit();


osThreadId insTaskHandle;
osThreadId robotTaskHandle;
osThreadId armTaskHandle;
osThreadId motorTaskHandle;
osThreadId daemonTaskHandle;
osThreadId cmdTaskHandle;

osThreadAttr_t ins_task_attr={
    .name = "ins_task",
    .stack_size = 1024,
    .priority = osPriorityAboveNormal,
};
osThreadAttr_t robot_task_attr={
    .name = " robot_task",
    .stack_size = 512,
    .priority = osPriorityNormal,
};
osThreadAttr_t motor_task_attr={
    .name = "motor_task",
    .stack_size = 512,
    .priority = osPriorityNormal,
};
osThreadAttr_t daemon_task_attr={
    .name = "daemon_task",
    .stack_size = 256,
    .priority = osPriorityNormal,
};
osThreadAttr_t arm_task_attr={
    .name = "arm_task",
    .stack_size = 512,
    .priority = osPriorityNormal,
};

osThreadAttr_t cmd_task_attr={
    .name = "cmd_task",
    .stack_size = 256,
    .priority = osPriorityAboveNormal,
};

void StartINSTASK(void *argument);
void StartMOTORTASK(void *argument);
void StartDAEMONTASK(void *argument);
void StartROBOTTASK(void *argument);
void StartARMTASK(void *argument);
void StartCMDTASK(void *argument);

void debug_wait()
{
    static bool debug_continue=false;
    while(!debug_continue)
    {
        osDelay(5);
    }
    debug_continue=false;

}

/**
 * @brief 初始化机器人任务,所有持续运行的任务都在这里初始化
 *
 */
void OSTaskInit()
{
    //用CMSIS-OS V2 API，应该更改为osThread.

    cmdTaskHandle = osThreadNew(StartCMDTASK, NULL, &cmd_task_attr);

    robotTaskHandle = osThreadNew(StartROBOTTASK, NULL, &robot_task_attr);

    motorTaskHandle = osThreadNew(StartMOTORTASK, NULL, &motor_task_attr);

    daemonTaskHandle = osThreadNew(StartDAEMONTASK, NULL, &daemon_task_attr);

    armTaskHandle = osThreadNew(StartARMTASK, NULL, &arm_task_attr);

    insTaskHandle = osThreadNew(StartINSTASK, NULL, &ins_task_attr);
}

void RobotInit()
{
    // 不要在初始化过程中使用中断和延时。若必须,则使用DWT_Delay()

    // 关闭中断,防止在初始化过程中发生中断
    __disable_irq();

    BSPInit();

    //APP层初始化
    Command_Init();
    Arm_Init();
    Chassis_Init();

    OSTaskInit(); // 创建基础任务

    // 初始化完成,开启中断
    __enable_irq();
}

inline void RobotTask()
{
    Chassis_Task();
    CalMasterTimer();
}

__attribute__((noreturn)) void StartINSTASK(void *argument)
{
    static float ins_start;
    static float ins_dt;
    INS_Init(); // 确保BMI088被正确初始化.
    LOGINFO("[freeRTOS] INS Task Start");
    for (;;)
    {
        // 1kHz
        ins_start = DWT_GetTimeline_ms();
        INS_Task();
        ins_dt = DWT_GetTimeline_ms() - ins_start;
        if (ins_dt > 1)
            LOGERROR("[freeRTOS] INS Task is being DELAY! dt = [%f]", &ins_dt);
        osDelay(1);
    }
}

__attribute__((noreturn)) void StartMOTORTASK(void *argument)
{
    static float motor_dt;
    static float motor_start;
    LOGINFO("[freeRTOS] MOTOR Task Start");
    for (;;)
    {
        motor_start = DWT_GetTimeline_ms();
        MotorControlTask();
        motor_dt = DWT_GetTimeline_ms() - motor_start;
        if (motor_dt > 1)
            LOGERROR("[freeRTOS] MOTOR Task is being DELAY! dt = [%f]", &motor_dt);
        osDelay(1);
    }
}

__attribute__((noreturn)) void StartDAEMONTASK(void *argument)
{
    static float daemon_dt;
    static float daemon_start;
    // BuzzerInit();
    // LOGINFO("[freeRTOS] Daemon Task Start");
    for (;;)
    {
        // // 100Hz
        // daemon_start = DWT_GetTimeline_ms();
        // DaemonTask();
        // BuzzerTask();
        // daemon_dt = DWT_GetTimeline_ms() - daemon_start;
        // if (daemon_dt > 10)
        //     LOGERROR("[freeRTOS] Daemon Task is being DELAY! dt = [%f]", &daemon_dt);
        osDelay(10);
    }
}

__attribute__((noreturn)) void StartROBOTTASK(void *argument)
{
    static float robot_dt;
    static float robot_start;
    LOGINFO("[freeRTOS] ROBOT core Task Start");
    for (;;)
    {
        robot_start = DWT_GetTimeline_ms();
        RobotTask();
        robot_dt = DWT_GetTimeline_ms() - robot_start;
        if (robot_dt > 2)
            LOGERROR("[freeRTOS] ROBOT core Task is being DELAY! dt = [%f]", &robot_dt);
        osDelay(2);
    }
}

//把机械臂任务独立开，因为它的舵机需要延时较久
__attribute__((noreturn)) void StartARMTASK(void *argument) {
    static float arm_dt;
    static float arm_start;
    static uint32_t prevTick;
    uint16_t taskperiod = 20;
    osDelay(50);
    for(;;) {
        prevTick = osKernelGetTickCount();  // 初始化上一次Tick计数
        arm_start = DWT_GetTimeline_ms();
        Arm_Task();
        arm_dt = DWT_GetTimeline_ms() - arm_start;
        osDelayUntil(prevTick+taskperiod);
    }

}

__attribute__((noreturn)) void StartCMDTASK(void *argument)
{
    osDelay(100);
    LOGINFO("[freeRTOS] CMD Task Start");
    Command_Execute();
    // Command_Test();
    for (;;)
    {
        Command_Test();
        // cmd_dt = DWT_GetTimeline_ms();
        //
        // cmd_dt = DWT_GetTimeline_ms() - cmd_start;
        // if (cmd_dt > 10)
        //     LOGERROR("[freeRTOS] ROBOT core Task is being DELAY! dt = [%f]", &cmd_dt);
        osDelay(10);
    }
}